﻿#region Using directives
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;
#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/b2Island.h
// Revision   : r113
// Change-Date: 2010-06-17
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/b2Island.cpp
// Revision   : r116
// Change-Date: 2010-06-21
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    internal struct Position
    {
        public Vector2 x;
        public float a;
    };

    internal struct Velocity
    {
        public Vector2 v;
        public float w;
    };

    public class Island
    {
        public Island()
        {
            contactSolver = new ContactSolver();
        }

        public void Reset(int bodyCapacity, int contactCapacity, int jointCapacity, ContactListener listener)
        {
            m_bodyCapacity = bodyCapacity;
            m_contactCapacity = contactCapacity;
            m_jointCapacity = jointCapacity;
            m_bodyCount = 0;
            m_contactCount = 0;
            m_jointCount = 0;

            m_listener = listener;

            if (m_bodies == null || m_bodies.Length < bodyCapacity) m_bodies = new Body[bodyCapacity];
            if (m_contacts == null || m_contacts.Length < contactCapacity) m_contacts = new Contact[contactCapacity];
            if (m_joints == null || m_joints.Length < jointCapacity) m_joints = new Joint[jointCapacity];

            if (m_velocities == null || m_velocities.Length < bodyCapacity) m_velocities = new Velocity[m_bodyCapacity];
            if (m_positions == null || m_positions.Length < bodyCapacity) m_positions = new Position[m_bodyCapacity];
        }

        ~Island()
        {
            m_positions = null;
            m_velocities = null;
            m_joints = null;
            m_contacts = null;
            m_bodies = null;
        }

	    public void Clear()
	    {
		    m_bodyCount = 0;
		    m_contactCount = 0;
		    m_jointCount = 0;
	    }

        static float linTolSqr = PhysicSettings.LinearSleepTolerance * PhysicSettings.LinearSleepTolerance;
        static float angTolSqr = PhysicSettings.AngularSleepTolerance * PhysicSettings.AngularSleepTolerance;

	    public void Solve(TimeStep step, Vector2 gravity, bool allowSleep)
        {
	        // Integrate velocities and apply damping.
	        for (int i = 0; i < m_bodyCount; ++i)
	        {
		        Body b = m_bodies[i];

		        if (b.Type != BodyType.DYNAMIC) continue;

		        // Integrate velocities.
		        b.m_linearVelocity += step.dt * (gravity + b.m_invMass * b.m_force);
		        b.m_angularVelocity += step.dt * b.m_invI * b.m_torque;

		        // Apply damping.
		        // ODE: dv/dt + c * v = 0
		        // Solution: v(t) = v0 * exp(-c * t)
		        // Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
		        // v2 = exp(-c * dt) * v1
		        // Taylor expansion:
		        // v2 = (1.0f - c * dt) * v1
		        b.m_linearVelocity *= MathHelper.Clamp(1.0f - step.dt * b.m_linearDamping, 0.0f, 1.0f);
		        b.m_angularVelocity *= MathHelper.Clamp(1.0f - step.dt * b.m_angularDamping, 0.0f, 1.0f);
	        }

            // Partition contacts so that contacts with static bodies are solved last.
            int i1 = -1;
            for (int i2 = 0; i2 < m_contactCount; ++i2)
            {
                Fixture fixtureA = m_contacts[i2].GetFixtureA();
                Fixture fixtureB = m_contacts[i2].GetFixtureB();
                Body bodyA = fixtureA.GetBody();
                Body bodyB = fixtureB.GetBody();
                bool nonStatic = bodyA.Type != BodyType.STATIC && bodyB.Type != BodyType.STATIC;
                if (nonStatic)
                {
                    ++i1;
                    Math.Swap<Contact>(ref m_contacts[i1], ref m_contacts[i2]);
                }
            }

	        // Initialize velocity constraints.
            ContactSolverDef solverDef = new ContactSolverDef();

            solverDef.contacts = m_contacts;
            solverDef.count = m_contactCount;
            solverDef.impulseRatio = step.dtRatio;
            solverDef.warmStarting = step.warmStarting;

            contactSolver.Reset(ref solverDef);
            
            contactSolver.InitializeVelocityConstraints();
            if (step.warmStarting)
            {
                contactSolver.WarmStart();
            }

	        for (int i = 0; i < m_jointCount; ++i)
	        {
		        m_joints[i].InitVelocityConstraints(step);
	        }

	        // Solve velocity constraints.
	        for (int i = 0; i < step.velocityIterations; ++i)
	        {
		        for (int j = 0; j < m_jointCount; ++j)
		        {
			        m_joints[j].SolveVelocityConstraints(step);
		        }
		        contactSolver.SolveVelocityConstraints();
	        }

	        // Post-solve (store impulses for warm starting).
            contactSolver.StoreImpulses();

	        // Integrate positions.
	        for (int i = 0; i < m_bodyCount; ++i)
	        {
		        Body b = m_bodies[i];

                if (b.Type == BodyType.STATIC)
                {
                    continue;
                }

		        // Check for large velocities.
		        Vector2 translation = step.dt * b.m_linearVelocity;
		        if (Vector2.Dot(translation, translation) > PhysicSettings.MaxTranslationSquared)
		        {
                    float ratio = PhysicSettings.MaxTranslation / translation.Length();
                    b.m_linearVelocity *= ratio;
                }

		        float rotation = step.dt * b.m_angularVelocity;
                if (rotation * rotation > PhysicSettings.MaxRotationSquared)
		        {
                    float ratio = PhysicSettings.MaxRotation / System.Math.Abs(rotation);
                    b.m_angularVelocity *= ratio;
                }

		        // Store positions for continuous collision.
		        b.m_sweep.c0 = b.m_sweep.c;
		        b.m_sweep.a0 = b.m_sweep.a;

		        // Integrate
		        b.m_sweep.c += step.dt * b.m_linearVelocity;
		        b.m_sweep.a += step.dt * b.m_angularVelocity;

		        // Compute new transform
		        b.SynchronizeTransform();

		        // Note: shapes are synchronized later.
	        }

	        // Iterate over constraints.
	        for (int i = 0; i < step.positionIterations; ++i)
	        {
                bool contactsOkay = contactSolver.SolvePositionConstraints(PhysicSettings.ContactBaumgarte);

		        bool jointsOkay = true;
		        for (int j = 0; j < m_jointCount; ++j)
		        {
                    bool jointOkay = m_joints[j].SolvePositionConstraints(PhysicSettings.ContactBaumgarte);
			        jointsOkay = jointsOkay && jointOkay;
		        }

		        if (contactsOkay && jointsOkay)
		        {
			        // Exit early if the position errors are small.
			        break;
		        }
	        }

	        Report(contactSolver.m_constraints);

	        if (allowSleep)
	        {
		        float minSleepTime = PhysicSettings.FLT_MAX;

//                float linTolSqr = b2Settings.LinearSleepTolerance * b2Settings.LinearSleepTolerance;
//                float angTolSqr = b2Settings.AngularSleepTolerance * b2Settings.AngularSleepTolerance;
 
		        for (Int32 i = 0; i < m_bodyCount; ++i)
		        {
			        Body b = m_bodies[i];
                    if (b.Type == BodyType.STATIC)
                    {
                        continue;
                    }

                    if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0)
                    {
                        b.m_sleepTime = 0.0f;
                        minSleepTime = 0.0f;
                    }


			        if ((b.m_flags & Body.BodyFlags.e_autoSleepFlag) == 0 ||
                        b.m_angularVelocity * b.m_angularVelocity > angTolSqr ||
                        Vector2.Dot(b.m_linearVelocity, b.m_linearVelocity) > linTolSqr)
                    {
				        b.m_sleepTime = 0.0f;
				        minSleepTime = 0.0f;
			        }
			        else
			        {
				        b.m_sleepTime += step.dt;
				        minSleepTime = System.Math.Min(minSleepTime, b.m_sleepTime);
			        }
		        }

		        if (minSleepTime >= PhysicSettings.TimeToSleep)
		        {
			        for (int i = 0; i < m_bodyCount; ++i)
			        {
				        m_bodies[i].Awake = false;
			        }
		        }
	        }
        }

        public void SolveTOI(TimeStep subStep, Body bodyA, Body bodyB)
        {
            ContactSolverDef solverDef = new ContactSolverDef();

            solverDef.contacts = m_contacts;
            solverDef.count = m_contactCount;
            solverDef.impulseRatio = subStep.dtRatio;
            solverDef.warmStarting = subStep.warmStarting;

            contactSolver.Reset(ref solverDef);

            // Solve position constraints.
            const float k_toiBaumgarte = 0.75f;
            for (int i = 0; i < subStep.positionIterations; ++i)
            {
                bool contactsOkay = contactSolver.SolveTOIPositionConstraints(k_toiBaumgarte, bodyA, bodyB);
                if (contactsOkay)
                {
                    break;
                }

                if (i == subStep.positionIterations - 1)
                {
                    i += 0;
                }
            }

#if false
            // Is the new position really safe?
            for (int i = 0; i < m_contactCount; ++i)
            {
                Contact c = m_contacts[i];
                Fixture fA = c.GetFixtureA();
                Fixture fB = c.GetFixtureB();

                Body bA = fA.GetBody();
                Body bB = fB.GetBody();

                int indexA = c.GetChildIndexA();
                int indexB = c.GetChildIndexB();

                DistanceInput input = new DistanceInput();
                input.proxyA.Set(fA.GetShape(), indexA);
                input.proxyB.Set(fB.GetShape(), indexB);
                input.transformA = bA.GetTransform();
                input.transformB = bB.GetTransform();
                input.useRadii = false;

                DistanceOutput output;
                SimplexCache cache;
                cache.count = 0;
                Collision.Distance(out output, ref cache, ref input);

                if (output.distance == 0 || cache.count == 3)
                {
                    cache.count += 0;
                }
            }
#endif

            // Leap of faith to new safe state.
            for (int i = 0; i < m_bodyCount; ++i)
            {
                m_bodies[i].m_sweep.a0 = m_bodies[i].m_sweep.a;
                m_bodies[i].m_sweep.c0 = m_bodies[i].m_sweep.c;
            }

            // No warm starting is needed for TOI events because warm
            // starting impulses were applied in the discrete solver.
            contactSolver.InitializeVelocityConstraints();

            // Solve velocity constraints.
            for (int i = 0; i < subStep.velocityIterations; ++i)
            {
                contactSolver.SolveVelocityConstraints();
            }

            // Don't store the TOI contact forces for warm starting
            // because they can be quite large.

            // Integrate positions.
            for (int i = 0; i < m_bodyCount; ++i)
            {
                Body b = m_bodies[i];

                if (b.Type == BodyType.STATIC)
                {
                    continue;
                }

                // Check for large velocities.
                Vector2 translation = subStep.dt * b.m_linearVelocity;
                if (Vector2.Dot(translation, translation) > PhysicSettings.MaxTranslationSquared)
                {
                    translation.Normalize();
                    b.m_linearVelocity = (PhysicSettings.MaxTranslation * subStep.inv_dt) * translation;
                }

                float rotation = subStep.dt * b.m_angularVelocity;
                if (rotation * rotation > PhysicSettings.MaxRotationSquared)
                {
                    if (rotation < 0.0f)
                    {
                        b.m_angularVelocity = -subStep.inv_dt * PhysicSettings.MaxRotation;
                    }
                    else
                    {
                        b.m_angularVelocity = subStep.inv_dt * PhysicSettings.MaxRotation;
                    }
                }

                // Integrate
                b.m_sweep.c += subStep.dt * b.m_linearVelocity;
                b.m_sweep.a += subStep.dt * b.m_angularVelocity;

                // Compute new transform
                b.SynchronizeTransform();

                // Note: shapes are synchronized later.
            }

            Report(contactSolver.m_constraints);
        }

	    public void Add(Body body)
	    {
		    System.Diagnostics.Debug.Assert(m_bodyCount < m_bodyCapacity);
		    body.m_islandIndex = m_bodyCount;
		    m_bodies[m_bodyCount++] = body;
	    }

	    public void Add(Contact contact)
	    {
		    System.Diagnostics.Debug.Assert(m_contactCount < m_contactCapacity);
		    m_contacts[m_contactCount++] = contact;
	    }

	    public void Add(Joint joint)
	    {
		    System.Diagnostics.Debug.Assert(m_jointCount < m_jointCapacity);
		    m_joints[m_jointCount++] = joint;
	    }

        public void Report(ContactConstraint[] constraints)
        {
            if (m_listener == null)
            {
                return;
            }

            for (Int32 i = 0; i < m_contactCount; ++i)
            {
                Contact c = m_contacts[i];

                ContactConstraint cc = constraints[i];

                ContactImpulse impulse = new ContactImpulse();
                for (Int32 j = 0; j < cc.pointCount; ++j)
                {
                    impulse.normalImpulses[j] = cc.points[j].normalImpulse;
                    impulse.tangentImpulses[j] = cc.points[j].tangentImpulse;
                }

                m_listener.PostSolve(c, impulse);
            }
        }

        public ContactListener m_listener;

        public Body[] m_bodies;
        public Contact[] m_contacts;
        public Joint[] m_joints;

        internal Position[] m_positions;
        internal Velocity[] m_velocities;

        public int m_bodyCount;
        public int m_jointCount;
        public int m_contactCount;

        public int m_bodyCapacity;
        public int m_contactCapacity;
        public int m_jointCapacity;

        private ContactSolver contactSolver;
    };
}
